

int t;
bool init;

Rotor rt1;
Rotor rt2;

int rt1_start;
int rt2_start;
int rt1_end;
int rt2_end;

int step;

void Main(string arguments){
    if(!init){GetBlocks(); return;}

    t ++;

    if(step == 0){
        bool a = rt1.TurnToPID(0);
        bool b = rt2.TurnToPID(0);
        if(a && b){
            step = 1;
        }
    }else if(step == 1){
        rt1_start = t;
        step = 2;
    }else if(step == 2){
        rt2.TurnToPID(0);
        if(rt1.TurnToPID(120)){
            rt1_end = t;
            rt2_start = t;
            step = 3;
        }
    }else if(step == 3){
        rt1.TurnToPID(120);
        if(rt2.TurnToPID(120)){
            rt2_end = t;
            step = 4;
        }
    }else if(step == 4){
        rt1.TurnToPID(120);
        rt2.TurnToPID(120);
    }

    Echo("模式: PID");
    Echo("P参数: "+rt1.turn_pid_p);
    Echo("I参数: "+rt1.turn_pid_i);
    Echo("D参数: "+rt1.turn_pid_d);
    Echo("T参数: "+rt1.turn_sum_t);
    Echo("=======================");
    Echo("R1开始:"+rt1_start.ToString());
    Echo("R1结束:"+rt1_end.ToString());
    Echo("R2开始:"+rt2_start.ToString());
    Echo("R2结束:"+rt2_end.ToString());
}

void GetBlocks(){
    rt1 = new Rotor(GridTerminalSystem.GetBlockWithName("Rotor1"));
    rt2 = new Rotor(GridTerminalSystem.GetBlockWithName("Rotor2"));
    init = true;
}

Program(){
	Runtime.UpdateFrequency = UpdateFrequency.Update1;
}

class Block
{
    // == 构造方法
    public Block(IMyTerminalBlock b){
        this._block = b;
    }

    // == 成员属性
    public IMyTerminalBlock _block; //方块
    public string Name{
        get { return this._block.CustomName; }
        set { this._block.CustomName = value; }
    }
    public string Data{
        get { return this._block.CustomData; }
        set { this._block.CustomData = value; }
    }
    public Vector3D Position{
        get { return this._block.GetPosition(); }
    }
    public bool ShowInTerminal{
        get { return this._block.ShowInTerminal; }
        set { this._block.ShowInTerminal = value; }
    }
    public bool ShowInInventory{
        get { return this._block.ShowInInventory; }
        set { this._block.ShowInInventory = value; }
    }

    // 静态方法
    public static List<Block> createListBlock(List<IMyTerminalBlock> blocks){
        List<Block> list = new List<Block>();
        foreach(IMyTerminalBlock b in blocks){
            list.Add(new Block(b));
        }
        return list;
    }
}

class LCD: Block
{
    // == 构造方法
    public LCD(IMyTerminalBlock b):base(b){
        this._block = b;
        this._lcd = b as IMyTextPanel;
    }
    public LCD(IMyTextPanel b):base(b){
        this._block = b;
        this._lcd = b as IMyTextPanel;
    }
    // == 静态方法
    public static List<LCD> createListBlock(List<IMyTextPanel> blocks){
        List<LCD> list = new List<LCD>();
        foreach(IMyTextPanel b in blocks){
            list.Add(new LCD(b));
        }
        return list;
    }

    // == 成员属性
    public IMyTextPanel _lcd; //lcd方块
    public string Title{
        get { return this._lcd.GetPublicTitle(); }
        set { this._lcd.WritePublicTitle(value); }
    }
    public string Text{
        get { return this._lcd.GetPublicText(); }
        set { this._lcd.WritePublicText(value); }
    }
    public bool ShowText{
        get { return this._lcd.GetValueBool("ShowTextOnScreen"); }
        set { this._lcd.SetValue("ShowTextOnScreen", value); }
    }

    // == 成员方法
    public void AppendText(string str){
        this._lcd.WritePublicText(str, true);
    }
}

class Rotor: Block
{
    // == 构造方法
    public Rotor(IMyTerminalBlock b):base(b){
        this._block = b;
        this._rotor = b as IMyMotorStator;
    }
    public Rotor(IMyMotorStator b):base(b){
        this._block = b;
        this._rotor = b as IMyMotorStator;
    }
    // == 静态方法
    public static List<Rotor> createListBlock(List<IMyMotorStator> blocks){
        List<Rotor> list = new List<Rotor>();
        foreach(IMyMotorStator b in blocks){
            list.Add(new Rotor(b));
        }
        return list;
    }
    // 角度转换
    public static double AngleToRad(double angle){
        return angle*Math.PI/180;
    }
    public static double RadToAngle(double rad){
        return rad*180/Math.PI;
    }
    public static double NormalizeAngle(double angle){
        if(angle > 360){
            return angle%360;
        }else if(angle < 0){
            return 360 + (angle%360);
        }else{
            return angle;
        }
    }


    // == 成员属性
    public IMyMotorStator _rotor; //转子方块
    public double TurnToAngleAccuracy = 0.1; //转动到位判定角度精度（角度）
    public double TurnToVelocityAccuracy = 0.01; //转动到位判定速度精度（RPM，圈每分钟）
    public double Angle{
        get { return Rotor.RadToAngle(this._rotor.Angle); }
    }
    public double AngleRad{
        get { return this._rotor.Angle; }
    }
    public double Velocity{
        get { return this._rotor.TargetVelocityRPM; }
        set { this._rotor.TargetVelocityRPM = (float)value; }
    }
    // public double VelocityRad{
    //     get { return this._rotor.TargetVelocity; }
    //     set { this._rotor.TargetVelocity = (float)value; }
    // }

    // == 成员方法

    // 转到某个角度
    // input angle 目标角度，使用角度而不是弧度
    // input mode 运动方式，1顺时针，-1逆时针，0自动
    // input velocity 指定速度，不指定默认用误差累加算法

    public double turn_sum_p = 5; //误差累加控制比例系数
    public double turn_sum_t = 20; //误差累加控制周期
    private List<double> turn_sum_diff_i = new List<double>();
    public bool TurnTo(double angle = 0, int mode = 0, double velocity = 0){
        angle = Rotor.NormalizeAngle(angle);

        if(mode == 0){
            if(angle > this.Angle){
                mode = angle - this.Angle > 180 ? -1 : 1;
            }else{
                mode = this.Angle - angle > 180 ? 1 : -1;
            }
        }

        //指定速度方式
        if(velocity != 0){
            this.Velocity = mode * velocity;
            return Math.Abs(angle - this.Angle) <= 0.5;
        }

        // diff是当前角度和目标角度的转动角度差，范围是0-1
        double diff = 0;
        if(mode == 1){
            if(angle > this.Angle){
                diff = angle - this.Angle;
            }else{
                diff = (360 - this.Angle) + angle;
            }
        }else if(mode == -1){
            if(this.Angle > angle){
                diff = this.Angle - angle;
            }else{
                diff = this.Angle + (360 - angle);
            }
        }
        diff /= 360;

        //对角度差进行逐帧累加，累加次数为 PID控制积分系数
        double diff_i = 0; //积分求和项
        if(this.turn_sum_diff_i.Count >= this.turn_sum_t){
            this.turn_sum_diff_i.Remove(this.turn_sum_diff_i[0]);
        }
        this.turn_sum_diff_i.Add(diff);
        foreach(double d in this.turn_sum_diff_i){
            diff_i += d;
        }

        //输出结果是 运动方向 * PID比例系数 * 角度差的逐帧累加和
        this.Velocity = mode * this.turn_sum_p*diff_i;

        return diff*360 <= this.TurnToAngleAccuracy && Math.Abs(this.Velocity) <= this.TurnToVelocityAccuracy;
    }
    // 转到某个弧度
    public bool TurnToRad(double angle = 0, int mode = 0, double velocity = 0){
        return this.TurnTo(Rotor.RadToAngle(angle), mode, velocity);
    }

    //PID算法的旋转
    public double turn_pid_p = 5; //比例系数
    public double turn_pid_i = 20; //积分系数(增加后降低动态误差，增加震荡)
    public double turn_pid_d = 20; //微分系数（增加后降低震荡）
    public double turn_pid_t = 30; //误差累加控制周期
    private List<double> turn_pid_diff_data = new List<double>();
    public bool TurnToPID(double angle = 0, int mode = 0, double velocity = 0){
        angle = Rotor.NormalizeAngle(angle);

        if(mode == 0){
            if(angle > this.Angle){
                mode = angle - this.Angle > 180 ? -1 : 1;
            }else{
                mode = this.Angle - angle > 180 ? 1 : -1;
            }
        }

        //指定速度方式
        if(velocity != 0){
            this.Velocity = mode * velocity;
            return Math.Abs(angle - this.Angle) <= 0.5;
        }

        // diff是当前角度和目标角度的转动角度差，范围是0-1
        double diff = 0;
        if(mode == 1){
            if(angle > this.Angle){
                diff = angle - this.Angle;
            }else{
                diff = (360 - this.Angle) + angle;
            }
        }else if(mode == -1){
            if(this.Angle > angle){
                diff = this.Angle - angle;
            }else{
                diff = this.Angle + (360 - angle);
            }
        }
        diff /= 360;

        //对角度差进行逐帧累加，累加次数为 PID控制积分系数
        double diff_i = 0; //积分求和项
        if(this.turn_pid_diff_data.Count >= this.turn_pid_t){
            this.turn_pid_diff_data.Remove(this.turn_pid_diff_data[0]);
        }
        this.turn_pid_diff_data.Add(diff);
        foreach(double d in this.turn_pid_diff_data){
            diff_i += d;
        }

        //输出结果是
        this.Velocity = mode*this.turn_pid_p*(diff + diff_i*this.turn_pid_i/turn_pid_t + (diff - this.turn_pid_diff_data[this.turn_pid_diff_data.Count-1])*this.turn_pid_d);

        return diff*360 <= this.TurnToAngleAccuracy && Math.Abs(this.Velocity) <= this.TurnToVelocityAccuracy;
    }

}

class Wheel: Block
{
    // == 构造方法
    public Wheel(IMyTerminalBlock b):base(b){
        this._block = b;
        this._wheel = b as IMyMotorSuspension;
    }
    public Wheel(IMyMotorSuspension b):base(b){
        this._block = b;
        this._wheel = b as IMyMotorSuspension;
    }
    // == 静态方法
    public static List<Wheel> createListBlock(List<IMyMotorSuspension> blocks){
        List<Wheel> list = new List<Wheel>();
        foreach(IMyMotorSuspension b in blocks){
            list.Add(new Wheel(b));
        }
        return list;
    }

    // == 成员属性
    public IMyMotorSuspension _wheel;
    public bool SteeringOnOff{ //是否允许转向
        get { return this._wheel.Steering; }
        set { this._wheel.Steering = value; }
    }
    public bool PropulsionOnOff{ //是否允许加速
        get { return this._wheel.Propulsion; }
        set { this._wheel.Propulsion = value; }
    }
    public bool InvertSteerOnOff{ //是否反向转向
        get { return this._wheel.InvertSteer; }
        set { this._wheel.InvertSteer = value; }
    }
    public bool InvertPropulsionOnOff{ //是否反向加速
        get { return this._wheel.InvertPropulsion; }
        set { this._wheel.InvertPropulsion = value; }
    }
    public double Power{ //功率
        get { return this._wheel.Power; }
        set { this._wheel.Power = (float)value; }
    }
    public double Strength{ //强度
        get { return this._wheel.Strength; }
        set { this._wheel.Strength = (float)value; }
    }
    public double Friction{ //摩擦力
        get { return this._wheel.Friction; }
        set { this._wheel.Friction = (float)value; }
    }
    public double SpeedLimit{ //速度限制，0~360，其中360为无限制
        get { return this._wheel.GetValueFloat("Speed Limit"); }
        set { this._wheel.SetValueFloat("Speed Limit", (float)value); }
    }
    public double Speed{ //加速越级值，0~1
        get { return this._wheel.GetValueFloat("Propulsion override"); }
        set { this._wheel.SetValueFloat("Propulsion override", (float)value); }
    }
    public double Steer{ //转向越级值，0~1
        get { return this._wheel.GetValueFloat("Steer override"); }
        set { this._wheel.SetValueFloat("Steer override", (float)value); }
    }
    public double Height{ //悬架高度，-1.5~1.3
        get { return this._wheel.Height; }
        set { this._wheel.Height = (float)value; }
    }

}

